/*
   This example code is in the Public Domain (or CC0 licensed, at your option.)

   Unless required by applicable law or agreed to in writing, this
   software is distributed on an "AS IS" BASIS, WITHOUT WARRANTIES OR
   CONDITIONS OF ANY KIND, either express or implied.
*/

#include <stdio.h>
#include <string.h>
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
#include "freertos/event_groups.h"
#include "esp_log.h"

#include "feather_esp.h"

#include "mpu9250.h"
#ifdef USE_MPU9250_DMP
#include "inv_mpu.h"
#include "inv_mpu_dmp_motion_driver.h"
#endif


/*
 **函數 : mpu9250_write_reg
 **功能 : 寫暫存器
 **輸入 : addr, data
 **輸出 : None
 **使用 : mpu9250_write_reg(addr, data);
 **
 */
static void mpu9250_write_reg(uint8_t addr, uint8_t data) 
{
    mpu9250_spi_cs_select();
    mpu9250_spi_rw_byte(addr);
    mpu9250_spi_rw_byte(data);
    mpu9250_spi_cs_unselect();
}

int mpu9250_write_regs(uint8_t addr, uint8_t waddr, uint8_t len, uint8_t* data) 
{
    uint8_t i = 0;

	mpu9250_spi_cs_select();
    mpu9250_spi_rw_byte(waddr);
    for(i = 0;i < len;i ++)
    {
        mpu9250_spi_rw_byte(data[i]);
    }
    mpu9250_spi_cs_unselect();
    return 0;
}

/*
 **函數 : mpu9250_read_reg
 **功能 : 讀暫存器
 **輸入 : addr
 **輸出 : data
 **使用 : mpu9250_read_reg(addr, &DeviceID);
 **
 */
void mpu9250_read_reg(uint8_t addr, uint8_t *data) 
{
    mpu9250_spi_cs_select();
    mpu9250_spi_rw_byte(0x80 | addr);
    *data = mpu9250_spi_rw_byte(0xFF);
    mpu9250_spi_cs_unselect();
}

/*
 **函數 : mpu9250_read_regs
 **功能 : 連續讀暫存器
 **輸入 : addr, *ReadBuf, Bytes
 **輸出 : None
 **使用 : mpu9250_read_regs(MPU6500_ACCEL_XOUT_H, ReadBuf, 14);
 **
 */
void mpu9250_read_regs(uint8_t addr, uint8_t *ReadBuf, uint8_t bytes) 
{
    uint8_t i = 0;

    mpu9250_spi_cs_select();
    mpu9250_spi_rw_byte(0x80 | addr);
    for (i = 0; i < bytes; i++)
    {
        ReadBuf[i] = mpu9250_spi_rw_byte(0xFF);
    }
    mpu9250_spi_cs_unselect();
}

/*
 **函數 : mpu9250_read_regs
 **功能 : 連續讀暫存器,对MPU9250_ReadRegs的重新封装,使用EMPL驱动时使用
 **輸入 : addr, addr, *ReadBuf, Bytes
 **輸出 : None
 **使用 : mpu9250_read_regs(MPU6500_ACCEL_XOUT_H, ReadBuf, 14);
 **
 */
int mpu9250_read_regs1( uint8_t addr,uint8_t raddr, uint8_t bytes,uint8_t *ReadBuf) 
{
	uint8_t i = 0;

	mpu9250_spi_cs_select();
	mpu9250_spi_rw_byte(0x80 | raddr);
	for(i = 0; i < bytes; i++)
    {
        ReadBuf[i] = mpu9250_spi_rw_byte(0x00);
    }
    mpu9250_spi_cs_unselect();
	return 0;
}

/*
 **函數 : mpu9250_check
 **功能 : MPU9250 Check
 **輸入 : None
 **輸出 : Status
 **使用 : Status = mpu9250_check();
 **
 */
uint8_t mpu9250_check(void) 
{
    uint8_t DeviceID = 0x00;

    /* MPU6500 */
    DeviceID = 0x00;
    mpu9250_read_reg(MPU6500_WHO_AM_I, &DeviceID);
    if (DeviceID != MPU6500_Device_ID )
        return 0;
    return 1;
    /* AK8975 */
    DeviceID = 0x00;
    mpu9250_write_reg(MPU6500_I2C_SLV0_ADDR, 0x8C);         // Set AK8963_I2C_ADDR = 7'b000_1100
    mpu9250_delay_us(10);
    mpu9250_write_reg(MPU6500_I2C_SLV0_REG, AK8963_WIA);     // Set Write Reg
    mpu9250_write_reg(MPU6500_I2C_SLV0_CTRL, 0x81);          // Start Read
    mpu9250_delay_ms(1);
    mpu9250_read_reg(MPU6500_EXT_SENS_DATA_00, &DeviceID);   // Read Data
    if (DeviceID != AK8963_Device_ID )
        return 0;

    return 1;
}

void mpu9250_reset(void) 
{
    mpu9250_write_reg(MPU6500_PWR_MGMT_1, 0x80);
}

void mpu9250_sleep(void) 
{
    mpu9250_write_reg(MPU6500_PWR_MGMT_1, 0x48);
    mpu9250_delay_ms(1);
    mpu9250_write_reg(MPU6500_PWR_MGMT_2, 0x3F);
}

void mpu9250_wakeup(void) 
{
    //mpu9250_write_reg(MPU6500_PWR_MGMT_1, 0x00);
    //mpu9250_delay_ms(1);
    //mpu9250_write_reg(MPU6500_PWR_MGMT_2, 0x00);
    //mpu9250_set_sensors(0);
}

int mpu9250_ak8963_spi_reads(uint8_t akm_addr, uint8_t reg_addr, uint8_t len, uint8_t* data) 
{
    uint8_t index = 0;
    uint8_t status = 0;
    uint32_t timeout = 0;
    uint8_t tmp = 0;

    tmp = akm_addr | MPU9250_I2C_READ;
    mpu9250_write_regs(MPU9250_SPIx_ADDR, MPU9250_I2C_SLV4_ADDR, 1, &tmp);
    mpu9250_delay_ms(1);
    while (index < len) 
    {
        tmp = reg_addr + index;
        mpu9250_write_regs(MPU9250_SPIx_ADDR, MPU9250_I2C_SLV4_REG, 1, &tmp);
        mpu9250_delay_ms(1);
        tmp = MPU9250_I2C_SLV4_EN;
        mpu9250_write_regs(MPU9250_SPIx_ADDR, MPU9250_I2C_SLV4_CTRL, 1, &tmp);
        mpu9250_delay_ms(1);

        do 
        {
            if(timeout++ > 50) 
            {
                return -2;
            }
#ifdef USE_MPU9250_DMP			
            mpu9250_read_regs1(MPU9250_SPIx_ADDR, MPU9250_I2C_MST_STATUS, 1, &status);
#else
			mpu9250_read_regs(MPU9250_I2C_MST_STATUS, &status, 1);
#endif
            mpu9250_delay_ms(2);
        } while ((status & MPU9250_I2C_SLV4_DONE) == 0);
#ifdef USE_MPU9250_DMP
        mpu9250_read_regs1(MPU9250_SPIx_ADDR, MPU9250_I2C_SLV4_DI, 1, data + index);
#else
		mpu9250_read_regs(MPU9250_I2C_SLV4_DI, data + index, 1);
#endif
        mpu9250_delay_ms(1);
        index++;
    }
    return 0;
}

int mpu9250_ak8963_spi_writes(uint8_t akm_addr, uint8_t reg_addr, uint8_t len, uint8_t* data) 
{
    uint32_t timeout = 0;
    uint8_t status = 0;
    uint8_t tmp = 0;
    uint8_t index = 0;

    tmp = akm_addr;
    mpu9250_write_regs(MPU9250_SPIx_ADDR, MPU9250_I2C_SLV4_ADDR, 1, &tmp);
    mpu9250_delay_ms(1);

    while (index < len) 
    {
        tmp = reg_addr + index;
        mpu9250_write_regs(MPU9250_SPIx_ADDR, MPU9250_I2C_SLV4_REG, 1, &tmp);
        mpu9250_delay_ms(1);
        mpu9250_write_regs(MPU9250_SPIx_ADDR, MPU9250_I2C_SLV4_DO, 1,data + index);
        mpu9250_delay_ms(1);
        tmp = MPU9250_I2C_SLV4_EN;
        mpu9250_write_regs(MPU9250_SPIx_ADDR, MPU9250_I2C_SLV4_CTRL, 1, &tmp);
        mpu9250_delay_ms(1);

        do 
        {
            if(timeout++ > 50)
                return -2;
#ifdef USE_MPU9250_DMP
            mpu9250_read_regs1(MPU9250_SPIx_ADDR, MPU9250_I2C_MST_STATUS, 1, &status);
#else
			mpu9250_read_regs(MPU9250_I2C_MST_STATUS, &status, 1);
#endif
            mpu9250_delay_ms(1);
        } while ((status & MPU9250_I2C_SLV4_DONE) == 0);
        if(status & MPU9250_I2C_SLV4_NACK)
            return -3;
        index++;
    }
    return 0;
}

static void mpu9250_spi_write(uint8_t addr, uint8_t waddr, uint8_t data) 
{
    mpu9250_write_reg(waddr, data);
}

static uint8_t mpu9250_spi_read(uint8_t addr, uint8_t reg_addr) 
{
    uint8_t data = 0;
    mpu9250_read_reg(addr, &data);
    return data;
}

static int mpu9250_ak8963_spi_write(uint8_t akm_addr, uint8_t reg_addr, uint8_t data) 
{
    mpu9250_ak8963_spi_writes(akm_addr, reg_addr, 1, &data);
    return 0;
}

/*
 **函數 : MPU9250_Init
 **功能 : 初始化 MPU9250
 **輸入 : None
 **輸出 : None
 **使用 : MPU9250_Init();
 **
 */
#define MPU9250_InitRegNum 16
void mpu9250_init1(void) 
{
    uint8_t i = 0;
    uint8_t MPU6500_Init_Data[MPU9250_InitRegNum][2] = 
    {
            { 0x80, MPU6500_PWR_MGMT_1 },     // Reset Device
            { 0x01, MPU6500_PWR_MGMT_1 },     // Clock Source
            { 0x18, MPU6500_GYRO_CONFIG },    // +-2000dps
            //{0x18, MPU6500_ACCEL_CONFIG},   // +-16G
            { 0x10, MPU6500_ACCEL_CONFIG },   // +-8G
            //{0x08, MPU6500_ACCEL_CONFIG},   // +-4G
            //{0x00, MPU6500_ACCEL_CONFIG},   // +-2G
            //{0x06, MPU6500_ACCEL_CONFIG_2}, // Set Acc Band Width 5Hz
            { 0x04, MPU6500_ACCEL_CONFIG_2 }, // Set Acc Band Width 20Hz
            //{0x07, MPU6500_PWR_MGMT_2},     // Enable Acc ,disable Gyro
            { 0x00, MPU6500_PWR_MGMT_2 },     // Enable Acc and Gyro
            { 0x06, MPU6500_CONFIG },         // 5HZ DLPF
            { 0x20, MPU6500_INT_PIN_CFG }, // INT PIN Config, The logic level for INT pin is active high
            //{0x11, MPU6500_INT_ENABLE},    // INT enable, raw sensor data reday
            { 0x00, MPU6500_INT_ENABLE },  // INT disable, raw sensor data reday
            { 0x00, MPU6500_FIFO_EN },       // ACCEL FIFO disable
            { 0x40, MPU6500_I2C_MST_CTRL },   // I2C Speed 348 kHz
            { 0x20, MPU6500_USER_CTRL },      // Enable I2C_MST

            // Set Slave to Read AK8963
            { 0x8C, MPU6500_I2C_SLV0_ADDR },  // AK8963_I2C_ADDR ( 7'b000_1100 )
            { 0x00, MPU6500_I2C_SLV0_REG },   // AK8963_WIA ( 0x00 )
            { 0x81, MPU6500_I2C_SLV0_CTRL },  // Enable
            { 0x01, MPU6500_I2C_MST_DELAY_CTRL } };

    mpu9250_write_reg(MPU6500_Init_Data[0][1], MPU6500_Init_Data[0][0]);
    mpu9250_delay_ms(200);
    mpu9250_write_reg(MPU6500_Init_Data[0][1], 0x00);
    mpu9250_delay_ms(10);

    for (i = 1; i < MPU9250_InitRegNum; i++) 
    {
        mpu9250_write_reg(MPU6500_Init_Data[i][1], MPU6500_Init_Data[i][0]);
        mpu9250_delay_ms(10);
    }

    static uint32_t cc;
    while(mpu9250_check() != 1 &&cc<0x0000FFFF)cc++;
}
static int16_t MPU9250_AK8963_ASA[3] = { 0, 0, 0 };

void mpu9250_init(void) 
{
    uint8_t data = 0, state = 0;
    uint8_t response[3] = { 0, 0, 0 };
    //Lower level hardware Init
    //SPIx_Init(pMPU9250);
    //EXTIx_Init(pMPU9250INT);
    //////////////////////////////////////////////////////////////////////////
    //MPU9250 Reset
    mpu9250_spi_write(MPU9250_SPIx_ADDR, MPU6500_PWR_MGMT_1, MPU9250_RESET);
    mpu9250_delay_ms(100);
    //MPU9250 Set Clock Source
    mpu9250_spi_write(MPU9250_SPIx_ADDR, MPU6500_PWR_MGMT_1,MPU9250_CLOCK_PLLGYROZ);
    mpu9250_delay_ms(1);
    //MPU9250 Set Interrupt
    mpu9250_spi_write(MPU9250_SPIx_ADDR, MPU6500_INT_PIN_CFG,MPU9250_INT_ANYRD_2CLEAR);
    mpu9250_delay_ms(1);
    mpu9250_spi_write(MPU9250_SPIx_ADDR, MPU6500_INT_ENABLE, 1);
    mpu9250_delay_ms(1);
    //MPU9250 Set Sensors
    mpu9250_spi_write(MPU9250_SPIx_ADDR, MPU6500_PWR_MGMT_2,MPU9250_XYZ_GYRO & MPU9250_XYZ_ACCEL);
    mpu9250_delay_ms(1);
    //MPU9250 Set SampleRate
    //SAMPLE_RATE = Internal_Sample_Rate / (1 + SMPLRT_DIV)
    mpu9250_spi_write(MPU9250_SPIx_ADDR, MPU9250_SMPLRT_DIV, SMPLRT_DIV);
    mpu9250_delay_ms(1);
    //MPU9250 Set Full Scale Gyro Range
    //Fchoice_b[1:0] = [00] enable DLPF
    mpu9250_spi_write(MPU9250_SPIx_ADDR, MPU9250_GYRO_CONFIG,(MPU9250_FSR_2000DPS << 3));
    mpu9250_delay_ms(1);
    //MPU9250 Set Full Scale Accel Range PS:2G
    mpu9250_spi_write(MPU9250_SPIx_ADDR, MPU9250_ACCEL_CONFIG,(MPU9250_FSR_8G << 3));
    mpu9250_delay_ms(1);
    //MPU9250 Set Accel DLPF
    data = mpu9250_spi_read(MPU9250_SPIx_ADDR, MPU9250_ACCEL_CONFIG2);
    data |= MPU9250_ACCEL_DLPF_41HZ;
    mpu9250_delay_ms(1);
    mpu9250_spi_write(MPU9250_SPIx_ADDR, MPU9250_ACCEL_CONFIG2, data);
    mpu9250_delay_ms(1);
    //MPU9250 Set Gyro DLPF
    mpu9250_spi_write(MPU9250_SPIx_ADDR, MPU9250_CONFIG,MPU9250_GYRO_DLPF_41HZ);
    mpu9250_delay_ms(1);
    //MPU9250 Set SPI Mode
    state = mpu9250_spi_read(MPU9250_SPIx_ADDR, MPU9250_USER_CTRL);
    mpu9250_delay_ms(1);
    mpu9250_spi_write(MPU9250_SPIx_ADDR, MPU9250_USER_CTRL,state | MPU9250_I2C_IF_DIS);
    mpu9250_delay_ms(1);
    state = mpu9250_spi_read(MPU9250_SPIx_ADDR, MPU9250_USER_CTRL);

    mpu9250_delay_ms(1);
    mpu9250_spi_write(MPU9250_SPIx_ADDR, MPU9250_USER_CTRL,state | MPU9250_I2C_MST_EN);
    mpu9250_delay_ms(1);
    //////////////////////////////////////////////////////////////////////////
    //AK8963 Setup
    //reset AK8963
    mpu9250_ak8963_spi_write(MPU9250_AK8963_I2C_ADDR, MPU9250_AK8963_CNTL2,MPU9250_AK8963_CNTL2_SRST);
    mpu9250_delay_ms(2);

    mpu9250_ak8963_spi_write(MPU9250_AK8963_I2C_ADDR, MPU9250_AK8963_CNTL,MPU9250_AK8963_POWER_DOWN);
    mpu9250_delay_ms(1);
    mpu9250_ak8963_spi_write(MPU9250_AK8963_I2C_ADDR, MPU9250_AK8963_CNTL,MPU9250_AK8963_FUSE_ROM_ACCESS);
    mpu9250_delay_ms(1);
    //
    //AK8963 get calibration data
    mpu9250_ak8963_spi_reads(MPU9250_AK8963_I2C_ADDR, MPU9250_AK8963_ASAX, 3, response);
    //AK8963_SENSITIVITY_SCALE_FACTOR
    //AK8963_ASA[i++] = (int16_t)((data - 128.0f) / 256.0f + 1.0f) ;
    MPU9250_AK8963_ASA[0] = (int16_t) (response[0]) + 128;
    MPU9250_AK8963_ASA[1] = (int16_t) (response[1]) + 128;
    MPU9250_AK8963_ASA[2] = (int16_t) (response[2]) + 128;
    mpu9250_delay_ms(1);
    mpu9250_ak8963_spi_write(MPU9250_AK8963_I2C_ADDR, MPU9250_AK8963_CNTL,MPU9250_AK8963_POWER_DOWN);
    mpu9250_delay_ms(1);
    //
    mpu9250_spi_write(MPU9250_SPIx_ADDR, MPU9250_I2C_MST_CTRL, 0x5D);
    mpu9250_delay_ms(1);
    mpu9250_spi_write(MPU9250_SPIx_ADDR, MPU9250_I2C_SLV0_ADDR,MPU9250_AK8963_I2C_ADDR | MPU9250_I2C_READ);
    mpu9250_delay_ms(1);
    mpu9250_spi_write(MPU9250_SPIx_ADDR, MPU9250_I2C_SLV0_REG,MPU9250_AK8963_ST1);
    mpu9250_delay_ms(1);
    mpu9250_spi_write(MPU9250_SPIx_ADDR, MPU9250_I2C_SLV0_CTRL, 0x88);
    mpu9250_delay_ms(1);
    //
    mpu9250_ak8963_spi_write(MPU9250_AK8963_I2C_ADDR, MPU9250_AK8963_CNTL, MPU9250_AK8963_CONTINUOUS_MEASUREMENT);
    mpu9250_delay_ms(1);

    //
    mpu9250_spi_write(MPU9250_SPIx_ADDR, MPU9250_I2C_SLV4_CTRL, 0x09);
    mpu9250_delay_ms(1);
    //
    mpu9250_spi_write(MPU9250_SPIx_ADDR, MPU9250_I2C_MST_DELAY_CTRL, 0x81);
    mpu9250_delay_ms(100);
}

void mpu9250_get_9axis_data(int16_t *accel, int16_t * gyro, int16_t * mag) 
{
    uint8_t data[22];
//	MPU9250_SPIx_Reads(MPU9250_SPIx_ADDR, MPU9250_ACCEL_XOUT_H, 22, data);
    mpu9250_read_regs(MPU9250_ACCEL_XOUT_H, data, 22);
    accel[0] = (data[0] << 8) | data[1];
    accel[1] = (data[2] << 8) | data[3];
    accel[2] = (data[4] << 8) | data[5];

    gyro[0] = (data[8] << 8) | data[9];
    gyro[1] = (data[10] << 8) | data[11];
    gyro[2] = (data[12] << 8) | data[13];

    if (!(data[14] & MPU9250_AK8963_DATA_READY)|| (data[14] & MPU9250_AK8963_DATA_OVERRUN)) 
    {
        return;
    }
    if (data[21] & MPU9250_AK8963_OVERFLOW) 
    {
        return;
    }
    mag[0] = (data[16] << 8) | data[15];
    mag[1] = (data[18] << 8) | data[17];
    mag[2] = (data[20] << 8) | data[19];

    //ned x,y,z
    mag[0] = ((long) mag[0] * MPU9250_AK8963_ASA[0]) >> 8;
    mag[1] = ((long) mag[1] * MPU9250_AK8963_ASA[1]) >> 8;
    mag[2] = ((long) mag[2] * MPU9250_AK8963_ASA[2]) >> 8;
}

#ifdef USE_MPU9250_DMP
static int8_t gyro_orientation[9] = 
{
 1, 0, 0,
 0, 1, 0,
 0, 0, 1
 };
/*{   0, 0, 1, 0, -1, 0, 1, 0, 0}*/;
static __inline unsigned short inv_row_2_scale(const signed char *row) 
{
    unsigned short b;
    if (row[0] > 0)
    b = 0;
    else if (row[0] < 0)
    b = 4;
    else if (row[1] > 0)
    b = 1;
    else if (row[1] < 0)
    b = 5;
    else if (row[2] > 0)
    b = 2;
    else if (row[2] < 0)
    b = 6;
    else
    b = 7;      // error
    return b;
}

static __inline unsigned short inv_orientation_matrix_to_scalar(const signed char *mtx) 
{
    unsigned short scalar;
    /*
     XYZ  010_001_000 Identity Matrix
     XZY  001_010_000
     YXZ  010_000_001
     YZX  000_010_001
     ZXY  001_000_010
     ZYX  000_001_010
     */
    scalar = inv_row_2_scale(mtx);
    scalar |= inv_row_2_scale(mtx + 3) << 3;
    scalar |= inv_row_2_scale(mtx + 6) << 6;
    return scalar;
}

int mpu9250_dmp_init(void) 
{
    //Init DMP
    int32_t s32Result = 0;
    struct int_param_s pInitParam;
    uint16_t u16GyroRate = 0;
    uint16_t u16GyroFsr = 0;
    uint8_t uint8_tAccelFsr = 0;
    uint16_t u16DmpFeatures = DMP_FEATURE_6X_LP_QUAT | DMP_FEATURE_SEND_RAW_ACCEL | DMP_FEATURE_SEND_CAL_GYRO;
    //uint16_t u16DmpFeatures = DMP_FEATURE_6X_LP_QUAT | DMP_FEATURE_TAP | DMP_FEATURE_ANDROID_ORIENT | DMP_FEATURE_SEND_RAW_ACCEL | DMP_FEATURE_SEND_CAL_GYRO | DMP_FEATURE_GYRO_CAL;

    s32Result = mpu_init(&pInitParam);
    if(s32Result)
	{
		printf("Could not initialize gyro (%d).\r\n",s32Result);
		return s32Result;
	}
    printf("initialize gyro success.\r\n");
    s32Result = mpu_set_sensors(INV_XYZ_GYRO | INV_XYZ_ACCEL | INV_XYZ_COMPASS);
    if (s32Result)
	{
		printf("Set sensors error (%d).\r\n",s32Result);
		return s32Result;
	}
    printf("Set sensors success.\r\n");
	/*
	s32Result = mpu9250_set_gyro_fsr(2000);
	if(s32Result)
	{
		printf("Set gyro fsr error (%d).\r\n",s32Result);
		return s32Result;
	}
    printf("Set gyro fsr success.\r\n");
	
	s32Result = mpu9250_set_accel_fsr(8);
	if(s32Result)
	{
		printf("Set accel fsr error (%d).\r\n",s32Result);
		return s32Result;
	}
    printf("Set accel fsr success.\r\n");
	*/
    s32Result = mpu_configure_fifo(INV_XYZ_GYRO | INV_XYZ_ACCEL);
    if (s32Result)
	{
		printf("configure fifo error (%d).\r\n",s32Result);
		return s32Result;//if disable,read data from regs
	}
    printf("configure fifo success.\r\n");
    
    s32Result = mpu_set_sample_rate(DEFAULT_MPU_HZ);
    if (s32Result)
	{
		printf("set sample rate error (%d).\r\n",s32Result);
		return s32Result;
	}
    printf("set sample rate success (%u).\r\n",DEFAULT_MPU_HZ);
#ifdef MPU9250  
    s32Result = mpu_set_compass_sample_rate(DEFAULT_MPU_HZ);
    if (s32Result)
	{
		printf("set compass sample rate error (%d).\r\n",s32Result);
		return s32Result;
	}
    printf("set compass sample rate success (%u).\r\n",DEFAULT_MPU_HZ);
#endif 
    s32Result = mpu_get_sample_rate(&u16GyroRate);
    if (s32Result)
	{
		printf("get sample rate error (%d).\r\n",s32Result);
		return s32Result;
	}
    printf("get sample rate success(%u).\r\n",u16GyroRate);
    
    s32Result = mpu_get_gyro_fsr(&u16GyroFsr);
	if (s32Result)
	{
		printf("get gyro fsr error (%d).\r\n",s32Result);
		return s32Result;
	}
    printf("get gyro fsr success(%u).\r\n",u16GyroFsr);

    s32Result = mpu_get_accel_fsr(&uint8_tAccelFsr);
	if (s32Result)
    {
		printf("get accel fsr error (%d).\r\n",s32Result);
		return s32Result;
	}
    printf("get accel fsr success(%u).\r\n",uint8_tAccelFsr);

    s32Result = dmp_load_motion_driver_firmware();
    if (s32Result)
	{
		printf("load motion driver firmware error (%d).\r\n",s32Result);
		return s32Result;
	}
    printf("load motion driver firmware success.\r\n");
    
    s32Result = dmp_set_orientation(inv_orientation_matrix_to_scalar(gyro_orientation));
    if (s32Result)
	{
		printf("set orientation error (%d).\r\n",s32Result);
		return s32Result;
	}
    printf("set orientation success.\r\n");
   
    s32Result = dmp_enable_feature(u16DmpFeatures);
    if (s32Result)
	{
		printf("enable feature error (%d).\r\n",s32Result);
		return s32Result;
	}
    printf("enable feature success(0x%04X).\r\n",u16DmpFeatures);
    
    s32Result = dmp_set_fifo_rate(DEFAULT_MPU_HZ);
    if (s32Result)
	{
		printf("set fifo rate error (%d).\r\n",s32Result);
		return s32Result;
	}
    printf("set fifo rate success (%u).\r\n",DEFAULT_MPU_HZ);
    
//    long * gyro_bias_init,accel_bias_init;
//    mpu9250_run_6500_self_test(gyro_bias_init,accel_bias_init,0);
//    dmp_set_gyro_bias(gyro_bias_init);
//    dmp_set_accel_bias(accel_bias_init);

	mpu9250_delay_ms(1);
    printf("run self test.\r\n");
	//run_self_test();

    s32Result = mpu_set_dmp_state(1);
    if (s32Result)
	{
		printf("set dmp state error (%d).\r\n",s32Result);
		return s32Result;
	}
    printf("set dmp state success.\r\n");
    
	mpu9250_delay_ms(100);
    return 0;
}

void mpu9250_get_dmp_data(int16_t *accel, int16_t * gyro, int16_t * mag,long *quat) 
{
    long dmpquat[4];
	static unsigned long timeStamp = 0;
	int16_t sensors, dmpGyro[3], dmpAccel[3], dmpMag[3];
	uint8_t more = 0;
    //int result;
	/* dmp data read*/
	do{
		dmp_read_fifo(dmpGyro, dmpAccel, dmpquat, &timeStamp, &sensors, &more);
        //dbg_log(9, "read dmp data result:0x%x.\r\n",result);
	}while(more!=0);
	/* gyro */
	if(sensors & INV_XYZ_GYRO)
		for(uint8_t i = 0; i < 3; i++)
			gyro[i] = dmpGyro[i];
	/* accel */
	if(sensors & INV_XYZ_ACCEL)
		for(uint8_t i = 0; i < 3; i++)
			accel[i] = dmpAccel[i];
	/* mag */
	if(!mpu_get_compass_reg(dmpMag, &timeStamp))
		for(uint8_t i = 0; i < 3; i++)
			mag[i] = dmpMag[i];

    /*四元数解姿态*/
    if(sensors & INV_WXYZ_QUAT)
    {
        quat[0] = dmpquat[0];
        quat[1] = dmpquat[1];
        quat[2] = dmpquat[2];
        quat[3] = dmpquat[3];
        //q0 = quat[0] / q30;
        //q1 = quat[1] / q30;
        //q2 = quat[2] / q30;
        //q3 = quat[3] / q30;
        //
        //Pitch  = asin(-2 * q1 * q3 + 2 * q0* q2)* 57.3 + Pitch_error; // pitch
        //Roll = atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2* q2 + 1)* 57.3 + Roll_error; // roll
        //Yaw = atan2(2*(q1*q2 + q0*q3),q0*q0+q1*q1-q2*q2-q3*q3) * 57.3 + Yaw_error;
        //
        //printf("Pitch :%5.1f,Roll  :%5.1f,Yaw   :%5.1f\r\n",Pitch,Roll,Yaw);
    }
}

#endif //end USE_MPU9250_DMP







